/**
  ******************************************************************************
  * @file    mathx_fit.c
  * @author  siyun.chen
  * @version v1.0.0
  * @date
  * @brief   适用于嵌入式端的C语言数学拓展库
  ******************************************************************************
  * Change Logs:
  * 2025-07-30      siyun.chen      first version
  ******************************************************************************
  *
  * MIT License
  *
  * Copyright (c) 2025 siyun.chen
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  * copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
  *
  * The above copyright notice and this permission notice shall be included in all
  * copies or substantial portions of the Software.
  *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  ******************************************************************************
  */
#include "mathx_fit.h"
#include <math.h>
#include <stddef.h>

#define EPSILON 1e-6

/**
 * @brief  一阶多项式最小二乘拟合（y=kx+b）
 * @param  x    输入数据点x轴数组
 * @param  y    输入数据点y轴数组
 * @param  n    数据点数量（n ≥ 2）
 * @param  k    输出参数：斜率k
 * @param  b    输出参数：截距b
 * @param  r2   输出参数：决定系数R²（可为NULL，为NULL时不计算决定系数）
 * @retval 0    成功
 * @retval -1   失败，参数错误或计算不稳定
 */
int mathx_fit_linear_f32(float *x, float *y, int n, float *k, float *b, float *r2)
{
    if (!x || !y || !k || !b || n < 2)
        return -1;

    double sum_x = 0.0f, sum_y = 0.0f, sum_xy = 0.0f, sum_x2 = 0.0f;

    for (int i = 0; i < n; i++)
    {
        sum_x  += x[i];
        sum_y  += y[i];
        sum_xy += x[i] * y[i];
        sum_x2 += x[i] * x[i];
    }

    double denominator = n * sum_x2 - sum_x * sum_x;

    // 检查分母是否为零或接近零
    if (fabs(denominator) < EPSILON)
        return -1;

    *k = (n * sum_xy - sum_x * sum_y) / denominator;
    *b = (sum_y - *k * sum_x) / n;

    if (r2 != NULL)
    {
        float sst = 0.0f, sse = 0.0f;
        float y_mean = sum_y / n;
        for (int i = 0; i < n; i++)
        {
            float y_pred = (*k) * x[i] + (*b);
            float dy = y[i] - y_mean;
            sst += dy * dy;
            dy = y[i] - y_pred;
            sse += dy * dy;
        }
        if (fabs(sst) < EPSILON)
            return -1;
        *r2 = 1.0f - sse / sst;
    }
    return 0;
}

/**
 * @brief  二阶多项式最小二乘拟合（y=ax²+bx+c）
 * @param  x    输入数据点x轴数组
 * @param  y    输入数据点y轴数组
 * @param  n    数据点数量（n ≥ 3）
 * @param  a    输出参数：二次项系数
 * @param  b    输出参数：一次项系数
 * @param  c    输出参数：常数项
 * @param  r2   输出参数：决定系数R²（可为NULL，为NULL时不计算决定系数）
 * @retval 0    成功
 * @retval -1   失败，参数错误或计算不稳定
 */
int mathx_fit_quadratic_f32(float *x, float *y, int n, float *a, float *b, float *c, float *r2)
{
    if (!x || !y || !a || !b || !c || n < 3)
        return -1;

    double sum_x = 0.0f, sum_x2 = 0.0f, sum_x3 = 0.0f, sum_x4 = 0.0f;
    double sum_y = 0.0f, sum_xy = 0.0f, sum_x2y = 0.0f;

    for (int i = 0; i < n; i++)
    {
        sum_x   += x[i];
        sum_x2  += x[i] * x[i];
        sum_x3  += x[i] * x[i] * x[i];
        sum_x4  += x[i] * x[i] * x[i] * x[i];
        sum_y   += y[i];
        sum_xy  += x[i] * y[i];
        sum_x2y += x[i] * x[i] * y[i];
    }

    // 构建矩阵行列式
    double D = n * (sum_x2 * sum_x4 - sum_x3 * sum_x3) 
             - sum_x * (sum_x * sum_x4 - sum_x2 * sum_x3)
             + sum_x2 * (sum_x * sum_x3 - sum_x2 * sum_x2);

    // 检查行列式是否接近零（计算不稳定）
    if (fabs(D) < EPSILON)
        return -1;

    // 计算逆矩阵系数
    double D_c = sum_y * (sum_x2 * sum_x4 - sum_x3 * sum_x3)
               - sum_xy * (sum_x * sum_x4 - sum_x2 * sum_x3)
               + sum_x2y * (sum_x * sum_x3 - sum_x2 * sum_x2);

    double D_b = n * (sum_xy * sum_x4 - sum_x2y * sum_x3)
               - sum_y * (sum_x * sum_x4 - sum_x2 * sum_x3)
               + sum_x2 * (sum_x * sum_x2y - sum_xy * sum_x2);

    double D_a = n * (sum_x2 * sum_x2y - sum_x3 * sum_xy)
               - sum_x * (sum_x * sum_x2y - sum_x2 * sum_xy)
               + sum_y * (sum_x * sum_x3 - sum_x2 * sum_x2);
    
    *a = D_a / D;
    *b = D_b / D;
    *c = D_c / D;

    if (r2 != NULL)
    {
        float sst = 0.0f, sse = 0.0f;
        float y_mean = sum_y / n;
        for (int i = 0; i < n; i++)
        {
            float y_pred = (*a) * x[i] * x[i] + (*b) * x[i] + (*c);
            float dy = y[i] - y_mean;
            sst += dy * dy;
            dy = y[i] - y_pred;
            sse += dy * dy;
        }
        if (fabs(sst) < EPSILON)
            return -1;
        *r2 = 1.0f - sse / sst;
    }
    return 0;
}
